#include <iostream>
#include <cmath>
#include <array>
#include <vector>
#include <fstream>
#include <sstream>
#include "../../include/api/grid_map.h"


struct Params {
    int numiters = 1000;
    int animate = 0;
    double dt = 0.1;
    double goal_tol = 0.25;
    double max_vel = 0.4;         // m/s
    double min_vel = 0.2;         // m/s
    double max_omega_vel = 0.5;   // rad/s
    double min_omega_vel = 0.1;   // rad/s
    double sensor_range_m = 0.25; // m
    double time_to_switch_goal = 5.0;  // sec (inactive for now)
    double sweep_resolution = 0.4;     // m
    std::vector<double> start_pos = {1, 2.5};  // Initial start position

    // You can add more member functions or modify as needed
};



std::vector<double> turn_right(std::vector<double>& pose, double yaw) {
    // goal_yaw=pose[2]-yaw
    pose[4] = 1 * sin(-yaw);  // omega(rad/s)
    std::cout << "turn_right" << pose[4] << std::endl;
    return pose;
}

std::vector<double> turn_left(std::vector<double>& pose, double yaw) {
    // goal_yaw=pose[2]-yaw
    pose[4] = 1 * sin(yaw);  // omega(rad/s)
    std::cout << "turn_left" << pose[4] << std::endl;
    return pose;
}



std::vector<double> slow_down(std::vector<double>& state, Params& params, double dv = 0.1) {
    if (state[3] > params.min_vel) {
        state[3] -= dv;
    }
    std::cout << "slow_down" << state[4] << std::endl;
    return state;
}



std::vector<double> motion(std::vector<double>& state, std::vector<double>& goal, Params& params, double initial_angle, double Heading_rad, double dt) {
    // state = {x(m), y(m), yaw(rad), v(m/s), omega(rad/s)}
    double dx = goal[0] - state[0];
    double dy = goal[1] - state[1];
    double goal_yaw = std::atan2(dy, dx);
    double K_theta = 1.5;
    double car_angle = Heading_rad - initial_angle;
    double angle_diff = std::sin(goal_yaw - car_angle);
    std::cout << "angle_diff " << angle_diff << std::endl;

    state[4] = K_theta * std::sin(goal_yaw - car_angle);  // omega(rad/s)
    state[2] = car_angle;  // yaw(rad)

    double dist_to_goal = std::sqrt(std::pow(goal[0] - state[0], 2) + std::pow(goal[1] - state[1], 2));
    double K_v = 0.1;
    state[3] += K_v * dist_to_goal;
    if (state[3] >= params.max_vel) state[3] = params.max_vel;
    if (state[3] <= params.min_vel) state[3] = params.min_vel;

    if (std::abs(angle_diff) > 3.1415926 / 4) {
        state[3] = state[3] * 0.0;
    }
    std::cout << "state ";
    for (auto& value : state) {
        std::cout << value << " ";
    }
    std::cout << std::endl;

    double dv = dt * state[3];
    state[0] += dv * std::cos(state[2]);  // x(m)
    state[1] += dv * std::sin(state[2]);  // y(m)

    return state;
}


std::vector<int> obstacle_check(std::vector<double>& pose, GridMap& gridmap, Params& params) {
    std::cout << "obstacle_check "<< std::endl;
    std::vector<std::vector<int>>& gmap = gridmap.gmap;
    std::cout << "obstacle_check "<< gmap.size()<< std::endl;
    int r = static_cast<int>(100 * params.sensor_range_m);
    std::vector<double> back = {pose[0] - r * cos(pose[2]), pose[1] - r * sin(pose[2])};
    std::vector<double> front = {pose[0] + r * cos(pose[2]), pose[1] + r * sin(pose[2])};
    std::vector<double> left = {pose[0] + r * cos(pose[2] + M_PI / 2), pose[1] + r * sin(pose[2] + M_PI / 2)};
    std::vector<double> right = {pose[0] - r * cos(pose[2] + M_PI / 2), pose[1] - r * sin(pose[2] + M_PI / 2)};

    std::vector<int> pi = {(int)pose[0], (int)pose[1]};
    std::vector<int> backi = {(int)back[0], (int)back[1]};
    std::vector<int> fronti = {(int)front[0], (int)front[1]};
    std::vector<int> lefti = {(int)left[0], (int)left[1]};
    std::vector<int> righti = {(int)right[0], (int)right[1]};

    std::vector<int> obstacle = {0, 0, 0, 0};

    /*for (int i = std::min(pi[0], fronti[0]); i <= std::max(pi[0], fronti[0]); ++i) {
        for (int j = std::min(pi[1], fronti[1]); j <= std::max(pi[1], fronti[1]); ++j) {
            int m = std::min(j, static_cast<int>(gmap.size()) - 1);
            int n = std::min(i, static_cast<int>(gmap[0].size()) - 1);
            if (gmap[m][n]) {
                obstacle[0] = 1;
            }
        }
    }
    
    for (int i = std::min(pi[0], backi[0]); i <= std::max(pi[0], backi[0]); ++i) {
        for (int j = std::min(pi[1], backi[1]); j <= std::max(pi[1], backi[1]); ++j) {
            int m = std::min(j, static_cast<int>(gmap.size()) - 1);
            int n = std::min(i, static_cast<int>(gmap[0].size()) - 1);
            if (gmap[m][n]) {
                obstacle[1] = 1;
            }
        }
    }

    for (int i = std::min(pi[0], lefti[0]); i <= std::max(pi[0], lefti[0]); ++i) {
        for (int j = std::min(pi[1], lefti[1]); j <= std::max(pi[1], lefti[1]); ++j) {
            int m = std::min(j, static_cast<int>(gm#include <fstream>ap.size()) - 1);
            int n = std::min(i, static_cast<int>(gmap[0].size()) - 1);
            if (gmap[m][n]) {
                obstacle[2] = 1;
            }
        }
    }

    for (int i = std::min(pi[0], righti[0]); i <= std::max(pi[0], righti[0]); ++i) {
        for (int j = std::min(pi[1], righti[1]); j <= std::max(pi[1], righti[1]); ++j) {
            int m = std::min(j, static_cast<int>(gmap.size()) - 1);
            int n = std::min(i, static_cast<int>(gmap[0].size()) - 1);
            if (gmap[m][n]) {
                obstacle[3] = 1;
            }
        }
    }*/
    std::cout << "obstacle_check "<< std::endl;
    
    return obstacle;
}
std::vector<double> collision_avoidance(std::vector<double>& state, GridMap& gridmap, Params& params) {
    
    std::vector<std::vector<double>> pose_grid_input = {{state[0], state[1]}};
    std::vector<std::vector<int>> pose_grid = gridmap.meters2grid(pose_grid_input);
    

    std::vector<double> pose_values = {pose_grid[0][0], pose_grid[0][1], state[2]};
    std::vector<int> boundary = obstacle_check(pose_values, gridmap, params);
#include <fstream>
    // Print the boundary values
    std::cout << "Right: " << boundary[0] << ", Front: " << boundary[1] << ", Left: " << boundary[2] << std::endl;

    if (boundary[0] || boundary[1]) {
        state = slow_down(state, params);
        state = turn_left(state, M_PI / 9.0);  // Approximately 20 degrees in radians
    } else if (boundary[2]) {
        state = slow_down(state, params);
        state = turn_right(state, M_PI / 9.0);  // Approximately 20 degrees in radians
    }

    return state;
}

struct Point {
    int x;
    int y;
};

// 
std::vector<Point> define_flight_area(const std::string& file_path) {
    //std::ifstream file("../src/api/coverage_path_planning/MapInformation/PolygonVertex.txt");
    std::ifstream file(file_path);
    if (!file.is_open()) {
        std::cerr << "Error opening file" << std::endl;
        return {};
    }

    // Creating an empty vector to store the read data
    std::vector<Point> polygonVertices;

    // Parsing each line of the file
    int x, y;
    while (file >> x >> y) {
        // Creating a Point object and adding it to the vector
        Point point = {x, y};
        polygonVertices.push_back(point);
    }

    file.close();

    // Assuming you have a FlightArea struct defined
    // Convert the vertices to the desired format (e.g., d#include <sstream>ividing by 100)
    std::vector<Point> flightAreaVertices;
    for (auto& point : polygonVertices) {
        flightAreaVertices.push_back({point.x / 100, point.y / 100});
    }

    return flightAreaVertices;
}

class IMUMessage {
public:
    double Roll_rad;
    double Pitch_rad;
    double Heading_rad;
    double RollSpeed;
    double PitchSpeed;
    double HeadingSpeed;
};

// 全局变量
double Roll_rad, Pitch_rad, Heading_rad, RollSpeed, PitchSpeed, HeadingSpeed;

void imu_callback(const std::string& topic_name, const IMUMessage& imu_msg, const double& time) {
    /* 
    std::cout << "Roll_rad: " << imu_msg.Roll_rad << ",\n"
              << "Pitch_rad: " << imu_msg.Pitch_rad << ",\n"
              << "Heading_rad: " << imu_msg.Heading_rad << ",\n"
              << "RollSpeed: " << imu_msg.RollSpeed << ",\n"
              << "PitchSpeed: " << imu_msg.PitchSpeed << ",\n"
              << "HeadingSpeed: " << imu_msg.HeadingSpeed << std::endl;
    */
    
    // 将数据保存到全局变量中
    Roll_rad = imu_msg.Roll_rad;
    Pitch_rad = imu_msg.Pitch_rad;
    Heading_rad = imu_msg.Heading_rad;
    RollSpeed = imu_msg.RollSpeed;
    PitchSpeed = imu_msg.PitchSpeed;
    HeadingSpeed = imu_msg.HeadingSpeed;
}

std::vector<std::vector<double>> read_CCP_data(const std::string& file_path) {
    std::ifstream file(file_path);

    if (!file.is_open()) {
        // 处理文件打开失败的情况
        std::cerr << "Error opening the file!" << std::endl;
        return {};
    }

    std::vector<std::vector<double>> data_array;

    // 读取文件内容
    std::string line;
    for (int i = 0; std::getline(file, line); ++i) {
        if (i >= 6) {
            std::istringstream iss(line);
            std::vector<double> row;
            double num;
            char comma;
            while (iss >> num) {
                row.push_back(num);
                if (iss >> comma && comma != ',') {
                    // 处理非逗号分隔的情况
                    std::cerr << "Error parsing the line!" << std::endl;
                    return {};
                }
            }
            data_array.push_back(row);
        }
    }

    // 前四个元素和后两个元素的处理
    data_array.erase(data_array.begin(), data_array.begin() + 4);
    data_array.erase(data_array.end() - 2, data_array.end());

    return data_array;
}


/*int main() {


    // Example usage of the define_flight_area function
    std::vector<Point> flightArea = define_flight_area();

    // Print the flight area vertices
    for (auto& point : flightArea) {
        std::cout << "x: " << point.x << ", y: " << point.y << std::endl;
    }


    GridMap gridmap_;
    std::vector<double>  pose = {0.0, 0.0, 0.0, 0.0, 0.0};
    double yaw = 0.0;

    pose = turn_right(pose, yaw);
    pose = turn_left(pose, yaw);


    std::vector<double>  state = {0.0, 0.0, 0.0, 0.0, 0.0};
    std::vector<double> goal = {1.0, 1.0};
    Params params;  // Replace 1.0 and 0.0 with the actual max_vel and min_vel values
    double initial_angle = 0.0;
    double Heading_rad = 0.0;
    double dt = 0.1;
    state = slow_down(state, params);
    state = motion(state, goal, params, initial_angle, Heading_rad, dt);

    std::vector<int> result = obstacle_check(pose, gridmap_, params);



    //std::vector<double> state = {2.0, 3.0, 1.5, 0.5, -0.00029189};
    std::vector<std::vector<int>> gmap;  // Replace this with the actual grid map
    collision_avoidance(state, gridmap_, params);
    
    return 0;
}*/

int main() {
    GridMap gridmap_;
    Params params;
    std::vector<double> state = {params.start_pos[0], params.start_pos[1], 0.0, 0.0, 0.0};

    // 提取 state 中的前两个元素作为 traj
    std::vector<double> traj(state.begin(), state.begin() + 2);

    // 输出 traj 中的值
    std::cout << "traj: ";
    for (const auto& value : traj) {
        std::cout << value << " ";
    }
    std::cout << std::endl;
    std::string flight_area_vertices_txt = "../src/api/coverage_path_planning/MapInformation/PolygonVertex.txt";
    std::vector<Point> flight_area_vertices = define_flight_area(flight_area_vertices_txt);
    std::vector<Point> posset;
    /*
    std::cout << "flight_area_vertices: ";
    for (const auto& point : flight_area_vertices) {
        std::cout << "(" << point.x << ", " << point.y << ") ";
    }
    std::cout << std::endl;

    
    posset.push_back({static_cast<int>(state[0]), static_cast<int>(state[1])});

    std::cout << "posset: ";
    for (const auto& point : posset) {
        std::cout << "(" << point.x << ", " << point.y << ") ";
    }
    std::cout << std::endl;*/
    
    std::string CCP_txt = "../src/api/coverage_path_planning/result/coverage_path_coordinate.txt";

    std::vector<std::vector<double>> data_array = read_CCP_data(CCP_txt);

    /*// 输出读取的数据
    for (const auto& point : data_array) {
        std::cout << point[0] << ", " << point[1] << std::endl;
    }*/

    
    return 0;
}








